#include "WPILib.h"
#include "Math.h"
#include "Robot2489.h"

//Get angle between two joints, in the kinect skeleton
double AngleXY(Skeleton::Joint origin, Skeleton::Joint measured, UINT8 mirrored){
    return (atan2((measured.y- origin.y), (mirrored) ? (origin.x - measured.x) : (measured.x - origin.x))*180/PI);
}

double AngleYZ(Skeleton::Joint origin, Skeleton::Joint measured, UINT8 mirrored){
    return (atan2((measured.y- origin.y), (mirrored) ? (origin.z - measured.z) : (measured.z - origin.z))*180/PI);
}


double CoerceToRange(double input, double inputMin, double inputMax, double outputMin, double outputMax) {
    // Determine the center of the input range
    double inputCenter = fabs(inputMax - inputMin) / 2 + inputMin;
    double outputCenter = fabs(outputMax - outputMin) / 2 + outputMin;
    // Scale the input range to the output range
    double scale = (outputMax - outputMin) / (inputMax - inputMin);
    // Apply the transformation
    double result = (input + -inputCenter) * scale + outputCenter;

    // Constrain to the result range
    return max(min(result, outputMax), outputMin);
}

void Robot2489::Autonomous(void) {
	myRobot->SetSafetyEnabled(false);
	ReadIOModule();
	timer->Start();
	while (IsAutonomous()) {
		//LineFollow();
		
		//Allow use of kill switch in autonomous
		ReadIOModule();
		
		if (EmergencyPause){
			StopArm();
			//SpinningWheelStop();
			ManipStop();
			myRobot->MecanumDrive_Cartesian(0, 0, 0);
			continue;
		}
		//Print angle of right arm
		//screen->PrintfLine(DriverStationLCD::kUser_Line1, "Angle: %f", kinect->GetSkeleton().GetShoulderRight(), kinect->GetSkeleton().GetWristRight(), false));
		
        //leftAxis = CoerceToRange(leftAngle, -70, 70, -1, 1);
        //rightAxis = CoerceToRange(rightAngle, -70, 70, -1, 1);
	}
}
